#ifndef DISABLE_DYNAMIXEL

#include "kernel.h"
#include "protocol.h"
#include "serial.h"
#include "timer.h"
#include "led.h"
#include "shared_memory.h"
#include "avr/interrupt.h"
#include "dynamixel.h"

// calculate a packet checksum
// checksum is ~(id + instruction + length + parameter1 + ... + parameterN)
// (see dynamixel documentation)
uint8_t calculateCheckSum(uint8_t id, uint8_t instruction, uint8_t const parameters[], uint8_t n)
{
    uint16_t sum = 0;
    uint8_t i;

    sum += id;
    sum += instruction;
    sum += (n+2);  // packet length is # of parameters +2

    for(i=0; i<n; i++)
        sum += parameters[i];

    return (uint8_t)(~sum);
}

uint8_t createChecksum(DXL_Packet *packet)
{
    packet->checksum = calculateCheckSum(packet->id, packet->instrErr, packet->params, packet->paramLength);
    return packet->checksum;
}

// send an arbitrary command to the servo(s)
// if dest != NULL the respnse packet is stored in the provided struct
// dest is zeroed-out if there was a timeout
void sendCommand(uint8_t id, uint8_t command, uint8_t parameters[], uint8_t paramLength, DXL_Packet *dest)
{
    DXL_Packet cmdPacket;
    uint8_t i;

    cmdPacket.header1 = DXL_HEADER1;
    cmdPacket.header2 = DXL_HEADER2;
    cmdPacket.id = id;
    cmdPacket.instrErr = command;
    cmdPacket.paramLength = paramLength;
    cmdPacket.checksum = calculateCheckSum(id, command, parameters, paramLength);

    for(i=0; i<paramLength && i < DXL_MAX_PARAMS; i++)
        cmdPacket.params[i] = parameters[i];

    lockSemaphore(&usart0_lock);
    setTxUSART0();
    transmitPacketUSART0(&cmdPacket);
    unlockSemaphore(&usart0_lock);

    // wait until the packet is completely transmitted
    // and then revert to listening on USART0
    while(!txQueue_USART0.empty)
        wait(USART0_TX_EVENT);
    setRxUSART0();
    fireEvent(COMMAND_PACKET_SENT_EVENT);

    // wait for a response if necessary
    if(dest!=NULL)
    {
        waitTimeout(STATUS_PACKET_RECEIVED_EVENT, DXL_REPLY_TIMEOUT);
        //wait(STATUS_PACKET_RECEIVED_EVENT);

        if(thread[currentThread].waitingFor & STATUS_PACKET_RECEIVED_EVENT)
        {
            // we successfully received a response packet
            lockSemaphore(&responsePacketLock);

            dest->header1 = responsePacket.header1;
            dest->header2 = responsePacket.header2;
            dest->id = responsePacket.id;
            dest->instrErr = responsePacket.instrErr;
            dest->paramLength = responsePacket.paramLength;

            // copy the response packet to the provided buffer
            for(i=0; i<dest->paramLength && i < DXL_MAX_PARAMS; i++)
            {
                dest->params[i] = responsePacket.params[i];
            }

            dest->checksum = responsePacket.checksum;

            unlockSemaphore(&responsePacketLock);
        }
        else
        {
#ifdef DEBUG
            lockSemaphore(&usart1_tx_lock);
            transmitStringUSART1("command timed out\n");
            unlockSemaphore(&usart1_tx_lock);
#endif

            // we timed out
            dest->header1 = 0x00;
            dest->header2 = 0x00;
            dest->id = 0x00;
            dest->paramLength = 0x00;
            dest->checksum = 0x00;
            dest->instrErr = 0x00;
        }
    } // dest!=null
} // sendCommand


// write one byte to the servo's address table
// if dest != NULL the respnse packet is stored in the provided struct
void writeByte(uint8_t id, uint8_t address, uint8_t data, DXL_Packet *dest)
{
    uint8_t parameters[2];
    parameters[0] = address;
    parameters[1] = data;

    sendCommand(id, DXL_WRITE_DATA, parameters, 2, dest);
}

// send a simple PING command to a servo and return TRUE/FALSE if the ping was received
BOOL ping(uint8_t id)
{
    DXL_Packet response;

    sendCommand(id, DXL_PING, NULL, 0, &response);

    if(validateResponsePacket(&response))
        return TRUE;
    else
        return FALSE;
}

// read bytes from the servo address table and store them in dest
// the result will be stored in dest->params
void readBytes(uint8_t id, uint8_t startingAddress, uint8_t numBytes, DXL_Packet *dest)
{
    uint8_t payload[2];

    payload[0] = startingAddress;
    payload[1] = numBytes;

    sendCommand(id, DXL_READ_DATA, payload, 0x02, dest);
}

// return TRUE if a packet is valid
// valid packets may have non-zero error codes, but the packet itself is valid
BOOL validateResponsePacket(DXL_Packet *packet)
{
    uint8_t checksum = calculateCheckSum(packet->id, packet->instrErr, packet->params, packet->paramLength);
    if(packet->header1 == DXL_HEADER1 && packet->header2 == DXL_HEADER2 && checksum == packet->checksum)
        return TRUE;
    else
        return FALSE;
}

#endif // DISABLE_DYNAMIXEL
